#ifndef PBABLOCKS_H
#define PBABLOCKS_H

#include <Eigen/Core>
#include <ceres/ceres.h>

#include "BaseTypes.hpp"
#include "../../util/mathfunc.h"

namespace Estimator
{
    
using namespace Eigen;

class PoseLocalParameterization : public ceres::LocalParameterization
{
  public:
    PoseLocalParameterization() noexcept  = default;
    virtual ~PoseLocalParameterization() = default;

    virtual bool Plus(const double *x,
                      const double *delta,
                      double *x_plus_delta) const override
    {
        Eigen::Map<const Eigen::Matrix<double, 3, 3, RowMajor>, 0, OuterStride<4>> RotaionMat(x, 3, 3);
        Eigen::Map<const Eigen::Matrix<double, 3, 1, ColMajor>, 0, InnerStride<4>> TransVec(x + 3, 3);
        Eigen::Map<const Eigen::Vector3d> delta_rot(delta, 3);
        Eigen::Map<const Eigen::Vector3d> delta_trans(delta + 3, 3);
        Eigen::Map< Eigen::Matrix<double,3,4, RowMajor> > xPlusTranslation(x_plus_delta);

        Eigen::Matrix3d dR, Jl;
        Exp_SE3(dR, Jl, delta_rot );

        xPlusTranslation.topLeftCorner<3,3>().noalias() = dR * RotaionMat;
        //xPlusTranslation.block<3,3>(0,0) = dR * RotaionMat;
        xPlusTranslation.topRightCorner<3,1>().noalias() = dR * TransVec + Jl*delta_trans;
        //xPlusTranslation.block<3,1>(0,3) = dR * TransVec + Jl*delta_trans;
        return true;
    }

    virtual bool ComputeJacobian(const double* x, double* jacobian) const override
    {
        // ceres::MatrixRef(jacobian, 12, 6) = ceres::Matrix::Zero(12,6);
        Eigen::Map< Eigen::Matrix<double, 12, 6, RowMajor> > Jacobian(jacobian);
        Jacobian.setZero();
        Jacobian.topLeftCorner<6,6>() = Eigen::Matrix<double,6,6,RowMajor>::Identity();
        return true;
    }

    virtual int GlobalSize() const override{
        return 12;
    }
    virtual int LocalSize() const override{
        return 6;
    }

    // get tgype information for debug or assert
    std::string typeInfo() const 
    {
        return "PoseVariable"; 
    }
                
};

class LandMarkLocalParameterization : public ceres::LocalParameterization
{
  public:
    LandMarkLocalParameterization() = default;
    virtual ~LandMarkLocalParameterization() = default;

    /*
        {x[0],x[1]} is the vector for cos(theta), sin(theta)
        {x[2],x[3],x[4]} is the direction in body frame..would be fixed during program.
    */
    virtual bool Plus(const double* x,
                      const double* delta,
                      double* x_plus_delta) const override
    {
        double cosdtheta = cos(delta[0]);
        double sindtheta = sin(delta[0]);
        x_plus_delta[0] =  x[0] * cosdtheta - x[1] * sindtheta;
        x_plus_delta[1] = x[0] * sindtheta + x[1] * cosdtheta;

        std::memcpy(x_plus_delta + 2, x + 2, 3*sizeof(double));
        return true;
    }

    virtual bool ComputeJacobian(const double* x, double* jacobian) const override
    {
        std::memset(jacobian, 0, 5*sizeof(double));
        Eigen::Map< Eigen::Matrix<double, 5 ,1, ColMajor> > Jacobians(jacobian);
        Jacobians(0,0) = 1.0;
        return true;
    }
    virtual int GlobalSize() const override{ return 5; }
    virtual int LocalSize() const override{ return 1; }
    std::string typeInfo() const { return "LandMarkVariable"; }            
};

class PoseBlock : public BlockBase<12, 6, Eigen::Matrix<double,3,4, RowMajor> >
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef BlockBase<12, 6, Eigen::Matrix<double,3,4, RowMajor> > BaseClass;
     // typedef BaseClass::parameter_t parameter_t;

    PoseBlock() noexcept = default;
    virtual ~PoseBlock() = default;

    PoseBlock(size_t id, const Eigen::Matrix<double, 3, 4, RowMajor> &Twb) noexcept
        : BaseClass(id)
    {
        setEstimate(Twb);
    }

    virtual void setEstimate(const BaseClass::parameter_t& Twb) override
    {
        memcpy(mParameters_, Twb.data(), Dimension*sizeof(double));
    }

    virtual parameter_t getEstimate() const override
    {
        return Map<const BaseClass::parameter_t>(mParameters_, 3, 4);
    }

    // get tgype information for debug or assert    
    virtual std::string typeInfo() const override
    {
        return "PoseVariable"; 
    }

};


// Pose Block with Translation only.
class PoseTransBlock : public BlockBase<3, 3, Eigen::Matrix<double, 3, 1> >
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef BlockBase<3, 3, Eigen::Matrix<double, 3, 1>> BaseClass;

    PoseTransBlock() noexcept = default;
    virtual ~PoseTransBlock() = default;

    PoseTransBlock(size_t id, const Eigen::Vector3d &trans) noexcept
        : BaseClass(id)
    {
        setEstimate(trans);
    }

    virtual void setEstimate(const BaseClass::parameter_t& Twb) override
    {
        assert(Dimension == Twb.rows());
        memcpy(mParameters_, Twb.data(), Dimension*sizeof(double));
    }

    virtual parameter_t getEstimate() const override
    {
        return Map<const BaseClass::parameter_t>(mParameters_, 3, 1);
    }

    // get tgype information for debug or assert    
    virtual std::string typeInfo() const override
    {
        return "PoseTranslationOnly"; 
    }

};

/*
        {x[0],x[1]} is the vector for cos(theta), sin(theta)
        {x[2],x[3],x[4]} is the direction in body frame..would be fixed during program.
    */

class LandMarkBlock : public BlockBase<5, 1, Eigen::Matrix<double, 5, 1, ColMajor> >
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef BlockBase<5, 1, Eigen::Matrix<double, 5, 1, ColMajor> > BaseClass;

    LandMarkBlock() noexcept = default;
    virtual ~LandMarkBlock() = default;

    LandMarkBlock(size_t id, BaseClass::parameter_t& landmark) noexcept
        : BaseClass(id)
    {
        setEstimate(landmark);
    }

    ///////////////////////////////////////////////////////////
    // override method in BlockBase class...

    virtual void setEstimate(const BaseClass::parameter_t& landmark) override {
        memcpy(mParameters_, landmark.data(), BaseClass::Dimension * sizeof(double));
    }
                
    virtual parameter_t getEstimate() const override {
        return Eigen::Map<const parameter_t>(mParameters_);
    }

    ///////////////////////////////////////////////////////////
    // override method in LocalParameter class...
    virtual std::string typeInfo() const override
    {
        return "LandMarkVariable"; 
    }
};

}

#endif